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1. INTRODUCTION 

Today, service robotics has become a real space for the application of robotics, with the possibility 
of evaluating the performance of intelligent systems in real cases [1, 2]. However, this also leads to 
the emergence of new sensing, control, and manipulation problems [3]. Active robotic sensors have today 
become a high-performance tool with great acceptance at commercial and military level [4, 5]. 
These are embedded systems equipped with sensors that provide specific primary data, from which 
a real-time processor produces information relevant to the tasks of the robot. This kind of sensors has 
promoted research in information-driven strategies for the development of tasks with robots, as well as the 
implementation of algorithms for digital signal processing and control schemes oriented to these sensors [6]. 

In the design of motion planning strategies for autonomous robots, this kind of sensors have become 
a fundamental tool. When environments are dynamic (a typical problem for service robots) it is necessary for 
the robot to be able to identify nearby obstacles in real time [7, 8]. Unstructured environments are more 
complex due to their dynamics and lack of knowledge of identifiable characteristics. In addition, not all 
obstacles are the same, and therefore the response of the robot must be different and correct for each of them. 
Depending on the application it is possible to use different kinds of sensors, but those capable of providing 
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visual information are the ones that provide more relevant information [9]. In this sense, systems with two 
cameras turn out to be more advantageous than systems with a single camera [8], since they provide 
information on the depth and orientation of the obstacle [6, 10-12]. 

In robotics, camera-supported optical sensors have been widely used in arm motion control. 
The camera provides the required feedback information in relation to the position of the objects 
to be manipulated. This strategy is known as visual servoing or vision-based robot control (VS) 
and is characterized by having as feedback information the image of a camera [13]. The aim is to support 
the robot’s decision making with eyes that take optical information from its own perspective and in parallel 
(separated by a certain distance) [11]. The distance between the robot and the obstacle can be determined 
depending on the distance between the obstacle positions in both images, and the focal distance of 
the cameras [14]. The field of vision can be increased considerably by adding a hyperboloid mirror or a conic 
mirror in front of the camera lenses, which provides an omnidirectional view to the cameras [15]. 

The reconstruction of 3D models from 2D perspectives (stereoscopic vision) is a strategy inspired 
by animal biology that allows the collection of three-dimensional information from the navigation 
environment. However, the process of generating 3D models is computationally expensive [16], and requires 
good camera calibration, making it very difficult to implement in real time on embedded systems [17]. 
In addition, the generation of 3D models is highly dependent on the quality of two-dimensional images, 
which are strongly affected by lighting conditions [18]. The computation of the distance to the obstacle takes 
into account the angular distance, the distance between the cameras and the pixels of the images [7, 11]. 
However, in many applications, it is not necessary to rebuild the entire environment, which considerably 
reduces the computational requirements [19]. In fact, the human brain does something similar by processing 
information from the eyes, only focusing on a portion of the entire image that the eye detects. 
This information can then be processed to find specific shapes [20, 21]. 

There are two strategies for estimating the distance to the obstacle in stereoscopic vision: active 
method and passive method [10, 22]. In the first case, the sensor system sends signals to the obstacle such as 
visible light or laser signals, which are then detected and analyzed [11]. The ability of these sensors to 
establish distances is superior to human vision, but they are also costly and complex to implement, and they 
have unresolved problems. For example, the laser delivers the distance of a single point. In fact, these 
methods do not determine the exact 3D positions of all points of the obstacle. Another negative aspect is their 
speed, they are very slow for real-time operation [23]. On the other hand, the passive methods estimate 
the location of the obstacle from the images of the environment captured by cameras [19]. They use digital 
processing on the images to estimate the distance. This passive strategy has the additional advantage of 
working with different setups (cameras, light conditions, and embedded hardware). It should be clarified that 
there are two problems that cannot be solved with this strategy: occlusions and overlapping of objects [24]. 

In order for the solutions to be real, it must be possible to massify them, and for this a low cost 
and high performance is essential [18, 23]. In this sense, processing algorithms must have very low 
computational cost in order to reduce processing time and hardware, while demonstrating to solve 
the problem. This paper attempts to address some of the critical problems of the strategy by maintaining 
a low cost, in particular reducing the impact of lighting on image quality, and improving the coincidence 
between 2D image points. 

The main idea of our strategy is to identify points of obstacles by means of a movement in 
the images based on bacterial interaction [25], these points are mapped in the planes of projection of 
the environment in order to establish the distance to the obstacle, all this without the need to make 
modifications to the environment [12]. The firmware used to control the hardware setup, as well as data 
acquisition and processing, is written in Python. We detail the methods and algorithms used for image 
processing and estimation of the distance to obstacles. The results presented are the product of real laboratory 
tests carried out on our robot. Our proposed bio-inspired algorithm for three-dimensional obstacle 
reconstruction and the resulting motion control scheme have a number of advantages over other methods that 
directly control the entire nonlinear system or rely on dynamic programming for planning [26]. 

The process of identifying the points belonging to the obstacle then becomes a search problem, 
and as such it can be solved by any search algorithm, in particular randomized uninformed search 
algorithms [27]. We use as a search algorithm a scheme based on bacterial interaction that the research group 
has successfully used in the solution of autonomous robot navigation problems [28, 29]. Under this scheme, 
a group of identical agents in an environment have limited local interaction and communication capabilities. 
From this local interaction with the environment, and according to their survival principle, the agents make 
decisions regarding displacement in the environment. The convergence of the algorithm is accelerated 
including quorum sensing (QS), that is, the areas are more attractive if there are many agents in them. 
In an equivalent way we use the algorithm for the agents to navigate the three-dimensional space until they 
reach the obstacle. 
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2. PROBLEM FORMULATION 

A robot is defined in a W workspace. Let W c R? be the closure of a contractible open set in space 
that has an open interior connected with obstacles that represent inaccessible volumes. Let O be a set 
of obstacles finite in number in which each O C O is closed and pairwise-disjoint. Let E c W be the free 
space in the environment, which is the open subset of W with the obstacles removed. 

The robot has two cameras that form an optical system of stereoscopic vision. This system is located 
in r(t) € R° and has R(t) € SO(3) orientation, where SO(3) denotes the special orthogonal group of dimension 
three with respect to a global frame of reference for every instant t > 0. To determine the position of the 
obstacles with respect to the robot, we define a relative frame of reference with respect to the axis of the two 
cameras as shown in Figure 1. We denote the two cameras by Left camera (Lc) and Right camera (Rc). 
The Lc and Rc centers are located at (-0.14, 0, 0) and (0.14, 0, O) in the relative reference framework. 
The distance between the cameras is b=0.14 + 0.14=0.28 m. 


| 
| 
| 
| 


Limited space for the creation and movement of bacteria 


Ni 
ee ee ee ee ce ee ee ee ee ee ee ee ee ee we we we we a ee A = ae ae ae a a a a eje am am am a a a a ao > a a a a af 


Left camera Right camera 





ARMOS TurtleBot 1 WE.............. 


Figure 1. Dimensions of the prototype with detailed location of the cameras, three-dimensional axes for 
the location of bacteria, and their limited space in the navigation environment (top view) 


The obstacles, indexed by i € H={ 1, 2, 3, ..., n} have unknown position x;(t), and can be moved in 
E over time. The position for the obstacle O; with respect to the global frame of reference in (1): 


xilt) = R(t) pit) + r(t) (1) 


where p;(t) corresponds to the position of the obstacle with respect to the frame of reference relative to 
the cameras. 

The cameras produce two parallel images at instant t with the location information p;(®). 
However, obstacles are not points, they are volumes whose surface is made up of a large number of points. 
We do not want to determine the position of all points of obstacles. Instead, we want to identify the position 
of a small group of points that will ideally move to the surface of the obstacles. We define a population of m 
bacteria in the space in which the robot may encounter obstacles when moving forward as shown in Figure 1. 

The initial position of each bacterium is random but known. From the images 
of the two cameras, we can establish trigonometric relationships for the three-dimensional position of each 
bacterium. If the bacteria are on the surface of the obstacle, then we can determine the distance to these 
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points of the obstacle as shown in Figure 2. We propose a search algorithm (obstacle search) in which 
bacteria move three-dimensionally according to local information detected in their 2D projections. In 
addition, the algorithm is accelerated according to the bacterial Quorum Sensing (QS), 1.e. large populations 
of bacteria in a space make the space more attractive to other bacteria. 
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Figure 2. Layout of elements in the test hardware and images resulting from the two cameras with details of 
an obstacle and two bacteria (top view) 


The m bacteria (or agents), all identical to each other, move in W searching for areas of great interest 
to them (for example, in search of food). The value of a given position is determined from local readings 
(local interaction with the medium) evaluated from its projection on 2D images. Each bacterium is defined 
by its position in the environment (2): 


V = (p) (2) 


where p is a point in 3-dimensional space (p E€ R°). The population density is evaluated using the distance 
between bacteria. Let (3): 


di; = d(V;, V;) (3) 


as the distance between bacteria V; and V;, which is calculated by an appropriate norm. The function used to 
evaluate the value of the region where the bacterium is found in the left and right projections considers 
the similarity of the neighboring pixels to the bacterium in the two projections (Figure 2). The mathematical 
expression in (4): 


È colors Tanen raii -R xpR+tiyR +/| 


where (xz, Yz) and (Xp, yr) are the coordinates of the left and right projections of the current bacterium, L¢:4i, 
yL+j) 18 the grey value at the left image at pixel (x,+/, y,+j) (in a similar way for the right image), N is 
the neighborhood around the projection of each bacterium, and |\(M)| is Sobel gradient norm on left 
and right projections (to penalize uniform regions). 

Bacterial QS is activated if the population density within a space is greater than a threshold value T 
called the quorum threshold. It is the parameter defining whether or not it has reached the quorum. 
The behaviors of bacteria (search in the environment) are coordinated by the following rule. If the bacterium 
V, c Wis located near to the bacterium V; C W, i.e. (5). 


dik < h (5) 


and the number of bacteria within the sphere with radius h/2 and origin in Vy is greater than T, then the value 
of the region increases for V;. 
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3. METHODOLOGY 

We initialize the bacterial population randomly within the field of action of the robot (red dotted line 
in the top view of Figure 1, 3 m along the x-axis, 2 m depth on the z-axis, and 2 m height above ground). 
The coordinates of each bacterium are defined with respect to the frame of reference relative to the cameras. 
The size of the population was taken as a performance variable parameter with values between 10 and 1000. 
The cameras are located on the robot at a height of 0.5 m from the ground. The origin of the frame of 
reference relative to these cameras is at this height, in the middle of the two cameras. The positive x-axis 
corresponds to the right side of the robot, the positive z-axis corresponds to the direction of advance of 


the robot, and the positive y-axis grows above the robot. The images of Le and R, are scaled to 800 X 600 
pixels. The projection of each bacterium i on the images is determined with the following equations (the 
position (0,0) of the image is in the upper left side): 


(x; +0.14)800 


p E 400 + 2z; tan(35) 
Left image: (y,)600 (6) 
= 300 -——- 
Yp 2z; tan(30) 
z (x;-0.14)800 
ae 2z; tan(35) 
Right image: (y,)600 (7) 
Yp = 300 — 2zi tan(30) 


where (x;, Yi, Zi) is the three-dimensional coordinate of the bacterium i, and (xp, Yp) is the two-dimensional 
coordinate of the bacterium projected in the image. 

The performance of the area adjacent to the bacteria at each projection is determined by (4). 
The bacteria move in the limited space according to this function. If the bacterium is on the obstacle surface, 
then it will have similar neighboring pixels in both projections (Figure 2, the illumination affects both 
cameras equally), and the function will assign a high value to the position of the bacterium. The more 
the neighboring pixels differ, the less value the function assigns. The position of the bacteria is updated with 
the gradient looking for the high values (movement of the bacteria). The QS forces the bacteria that are slow 
to find the obstacle surface to move towards the large groups of bacteria. A bacterium that does not appear in 
any of the projections obtains the lowest position value (it is outside the robot's range of vision). 


4. FINDINGS 

We evaluate the performance of the strategy with different configurations varying the bacteria 
population, the QS threshold and the correlation window used in the denominator of the evaluation function. 
A larger number of bacteria allows for reconstructing larger portions of the obstacles without significantly 
influencing the computational cost of the algorithm. The QS threshold reduces the convergence time when it 
does not exceed the range of 100, above this value, does not have a significant effect. The most important 
effect was observed in the size of the correlation window of the function, which greatly affects the bacteria's 
ability to locate the obstacle. Large values improve the behavior but considerably increase the computational 
cost. Figure 3 shows the result of one of the laboratory tests. 
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Figure 3. Image of the left camera converted to grayscale, scaled to 800x600 and with the bacteria 
overlapped in its final position, most of them on the obstacle 
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We perform more than 50 laboratory tests with different obstacles and more or less constant lighting 
conditions for a human indoor environment (the day with natural lighting and night with LED type lighting). 
The distances from the objects to the robot were established in a straight line between 0.3 and 2 m. 
The accuracy of the distance values determined by the optical sensor was established by comparison with 
the actual value, measured in the setup with a tape measure. These results were related to the distance of 
the obstacle. Figure 4 shows these percentages of accuracy with respect to the estimated distance. 


Accuracy (%) 
Estimated distance vs. actual distance 





0 0.5 1 1.5 2 
Distance to obstacles [m] 


Figure 4. Percentages of accuracy with respect to the estimated distance 


Our intention is to use the strategy to identify obstacles in the environment, and with 
this information to coordinate the movement of the robot. The proposed motion planning strategy based 
on the detection and stereoscopic identification of obstacles considers three elements: capture and 
pre-processing of images, determination of obstacles and application of motion policies according to 
the information feedback as shown in Figure 5. 


Start Navigation 


Capture of images by cameras 





Elimination of irrelevant regions: 
Backgound and ground plane 


Clustering and seqmentation 
of obstacle regions based on 
pixels and bacterial algorithm 


Parameters extraction: 
distance to obstacle, position and size of obstacle 


Movement strategy 


Figure 5. General scheme of the proposed motion planning strategy based on the stereoscopic detection 
of obstacles 
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5. CONCLUSIONS 

In this paper, we propose a low computational cost strategy for obstacle detection by stereoscopic 
vision. This strategy is intended to be used for planning the movement of autonomous service robots in 
dynamic indoor environments. The high performance and low computational cost were achieved by avoiding 
a complete reconstruction of the environment. Instead, we performed the calculation of about 100 points 
corresponding to the surface of the obstacles. These points were found using a search algorithm based on 
bacterial behavior. The bacteria defined in the 2D projections of the cameras move in the three-dimensional 
space looking for similar neighboring regions in their projections. The algorithm converges with most 
bacteria on the obstacles. In the experiments carried out it was possible to verify percentages of accuracy 
with respect to the obstacle distance higher than 95% and low computational consumption, making it useful 
for embedded implementations. However, for distances less than 0.8 m the scheme presents very high error 
values, suggesting a greater work of tuning the bacterial interaction scheme. 
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